#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <uartTask.h>

/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"
#include "projdefs.h"
#include "semphr.h"

/* include files. */
#include "vtUtilities.h"
#include "vtI2C.h"
#include "LCDtask.h"
#include "i2cTemp.h"
#include "I2CTaskMsgTypes.h"
#include "graphTask.h"

/* *********************************************** */
// definitions and data structures that are private to this file
// Length of the queue to this task
extern xQueueHandle GraphQueue;
portTickType xLastWakeTime;
const portTickType xFrequency = 500 / portTICK_RATE_MS;
static void VoltageTimerCallback( xTimerHandle pxTimer );
xTimerHandle VoltageTimer;
uint8_t SlaveAddress = 0x24;
#define vtTempQLen 10 
// actual data structure that is sent in a message
typedef struct __vtTempMsg {
        uint8_t msgType;
        uint8_t length;  // Length of the message to be printed
        uint8_t buf[vtTempMaxLen+1]; // On the way in, message to be sent, on the way out, message received (if any)
} vtTempMsg;

// I have set this to a large stack size because of (a) using printf() and (b) the depth of function calls
//   for some of the i2c operations     -- almost certainly too large, see LCDTask.c for details on how to check the size
#define baseStack 3
#if PRINTF_VERSION == 1
#define i2cSTACK_SIZE           ((baseStack+5)*configMINIMAL_STACK_SIZE)
#else
#define i2cSTACK_SIZE           (baseStack*configMINIMAL_STACK_SIZE)
#endif

// end of defs
/* *********************************************** */

/* The i2cTemp task. */
static portTASK_FUNCTION_PROTO( vi2cTempUpdateTask, pvParameters );

/*-----------------------------------------------------------*/
// Public API
void vStarti2cTempTask(vtTempStruct *params,unsigned portBASE_TYPE uxPriority, vtI2CStruct *i2c,vtLCDStruct *lcd)
{
        // Create the queue that will be used to talk to this task
        if ((params->inQ = xQueueCreate(vtTempQLen,sizeof(vtTempMsg))) == NULL) {
                VT_HANDLE_FATAL_ERROR(0);
        }
        /* Start the task */
        portBASE_TYPE retval;
        params->dev = i2c;
        params->lcdData = lcd;
        if ((retval = xTaskCreate( vi2cTempUpdateTask, ( signed char * ) "i2cTemp", i2cSTACK_SIZE, (void *) params, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
                VT_HANDLE_FATAL_ERROR(retval);
        }
}

portBASE_TYPE SendTempTimerMsg(vtTempStruct *tempData,portTickType ticksElapsed,portTickType ticksToBlock)
{
        if (tempData == NULL) {
                VT_HANDLE_FATAL_ERROR(0);
        }
        vtTempMsg tempBuffer;
        tempBuffer.length = sizeof(ticksElapsed);
        if (tempBuffer.length > vtTempMaxLen) {
                // no room for this message
                VT_HANDLE_FATAL_ERROR(tempBuffer.length);
        }
        memcpy(tempBuffer.buf,(char *)&ticksElapsed,sizeof(ticksElapsed));
        tempBuffer.msgType = TempMsgTypeTimer;
        return(xQueueSend(tempData->inQ,(void *) (&tempBuffer),ticksToBlock));
}

portBASE_TYPE SendTempValueMsg(vtTempStruct *tempData,uint8_t msgType,uint8_t value,portTickType ticksToBlock)
{
        vtTempMsg tempBuffer;

        if (tempData == NULL) {
                VT_HANDLE_FATAL_ERROR(0);
        }
        tempBuffer.length = sizeof(value);
        if (tempBuffer.length > vtTempMaxLen) {
                // no room for this message
                VT_HANDLE_FATAL_ERROR(tempBuffer.length);
        }
        memcpy(tempBuffer.buf,(char *)&value,sizeof(value));
        tempBuffer.msgType = msgType;
        return(xQueueSend(tempData->inQ,(void *) (&tempBuffer),ticksToBlock));
}

// End of Public API
/*-----------------------------------------------------------*/
int getMsgType(vtTempMsg *Buffer)
{
        return(Buffer->msgType);
}
uint8_t getValue(vtTempMsg *Buffer)
{
        uint8_t *ptr = (uint8_t *) Buffer->buf;
        return(*ptr);
}
void clearValue(vtTempMsg *Buffer)
{
	int i;
	for(i=0;i<sizeof(Buffer->buf);i++)
		Buffer->buf[i] = 0;
}

// I2C commands for the temperature sensor
        const uint8_t i2cCmdInit[]= {0xAC,0x00};
        const uint8_t i2cCmdStartConvert[]= {0xEE};
        const uint8_t i2cCmdStopConvert[]= {0x22};
        const uint8_t i2cCmdReadVals[]= {0x55};
        const uint8_t i2cCmdReadCnt[]= {0xA8};
        const uint8_t i2cCmdReadSlope[]= {0xA9};
// end of I2C command definitions

// Definitions of the states for the FSM below
const uint8_t fsmStateInit1Sent = 0;
const uint8_t fsmStateInit2Sent = 1;
const uint8_t fsmStateTempRead1 = 2;
const uint8_t fsmStateTempRead2 = 3;
const uint8_t fsmStateTempRead3 = 4;
// This is the actual task that is run
static portTASK_FUNCTION( vi2cTempUpdateTask, pvParameters )
{
        float temperature = 0.0;
        float countPerC = 100.0, countRemain=0.0;
        // Get the parameters
        vtTempStruct *param = (vtTempStruct *) pvParameters;
        // Get the I2C device pointer
        vtI2CStruct *devPtr = param->dev;
        // Get the LCD information pointer
        vtLCDStruct *lcdData = param->lcdData;
        // String buffer for printing
        char lcdBuffer[vtLCDMaxLen+1];
        // Buffer for receiving messages
        vtTempMsg msgBuffer;
        uint8_t currentState;
			VoltageTimer = xTimerCreate
		          (  /* Just a text name, not used by the RTOS kernel. */
                     (const signed char*)"GraphTimer",
                     /* The timer period in ticks. */
                     ( 10000 / portTICK_RATE_MS  ),
                     /* The timers will auto-reload themselves when they expire. */
                     pdTRUE,
                     /* Assign each timer a unique id equal to its array index. */
                     ( void * ) param,
                     /* Each timer calls the same callback when it expires. */
                     VoltageTimerCallback
                   );

		if( xTimerStart( VoltageTimer, 0 ) != pdPASS )
             {
                 /* The timer could not be set into the Active state. */
						 
             }
		/* Initialize the LCD and set the initial colors */
	GLCD_Init();
	GLCD_Clear(Red);
	GLCD_SetTextColor(Green);
	GLCD_SetBackColor(Red);
	GLCD_Clear(Red);
		 int flag = 0;
		 unsigned char UpperByte = 0;
		 unsigned char LowerByte = 0;
		 unsigned char x_=0;
        // Assumes that the I2C device (and thread) have already been initialized
        // Like all good tasks, this should never exit
        for(;;)
        {
                // Wait for a message from either a timer or from an I2C operation
                if (xQueueReceive(param->inQ,(void *) &msgBuffer,0) != pdTRUE) {
                        VT_HANDLE_FATAL_ERROR(0);
                }

                // Now, based on the type of the message and the state, we decide on the new state and action to take
                switch(getMsgType(&msgBuffer)) {

                case g8I2CMsgTypeVoltRead: {
						
					    unsigned char distance = getValue(&msgBuffer);
						unsigned char buff[4];
						print_hex_packet(&distance,sizeof(distance),buff);
						GLCD_DisplayString(x_,10,0.3,(unsigned char*)buff);
						x_++;
						//sample =response;
						//Byte = 0;
						
						//sample++;
						//status = 0;
						
						if(x_==30){
						 GLCD_Clear(Red);
						 x_ = 0;
						}
//						if(flag == 2){
//						   if( GraphQueue != 0 )
//    					{
//        				//LowerByte = (unsigned char)getValue(&msgBuffer);
//						//struct I2cPacket v;
//						//v.value = (int)getValue(&msgBuffer);//(256 * UpperByte + LowerByte);
//						//v.value = UpperByte << 8;
//						//v.value += LowerByte;
//						//v.value = v.value & 0x3FFF;
//
//		
//						//struct I2cPacket * msg = &v;
//       					 if( xQueueSend( GraphQueue, ( void * ) &msg, ( portTickType ) 10 ) != pdPASS )
//        						{
//           						 // Failed to post the message
//	        						}
//			
//
//   		 				}
//						//clearValue(&msgBuffer);
//						flag = 0;
//						}else{
//						 	UpperByte = (unsigned char)getValue(&msgBuffer);
//						}
//                        
                        break;
                }
                case TempMsgTypeTimer: {
                      
                                if (vtI2CEnQ(devPtr,g8I2CMsgTypeVoltRead,SlaveAddress,0,0,1) != pdTRUE) {
                                        VT_HANDLE_FATAL_ERROR(0);
                                }
								//flag++;

								 
					

                        break;
                }		
                      
                default: {
                        VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer));
                        break;
                }
                }



        }
}


static void VoltageTimerCallback( xTimerHandle pxTimer )
 {
	
	  vtTempStruct *ptr = (vtTempStruct *) pvTimerGetTimerID(pxTimer);
		  
	  if (SendTempTimerMsg(ptr,( ( portTickType ) 0 / portTICK_RATE_MS),0) == errQUEUE_FULL) {
			// Here is where you would do something if you wanted to handle the queue being full
			VT_HANDLE_FATAL_ERROR(0);
		}
		

}